/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file      obtein_mineral_behaviour.c/h
  * @brief     
  * @note       
  * @history
  *  V1.0.0            2022       江宽      
  @verbatim
  ==============================================================================
  ==============================================================================
  @endverbatim
  *************************(C) COPYRIGHT 2022 LionHeart*************************
  */

#include "obtein_mineral_behaviour.h"
#include "obtein_mineral_task.h"
#include "calibrate_task.h"
#include "detect_task.h"
#include "gimbal_task.h"
#include "tim.h"
static engineer_behaviour_e engineer_behaviour = engineer_ZERO_FORCE;
       engineer_work_e engineer_work;
/**
  * @brief          通过判断限位来判断夹具是否到达极限位置
  * @param          对应轴的角速度，单位rad/s
  * @param          计时时间，到达GIMBAL_CALI_STEP_TIME的时间后归零
  * @param          记录的角度 rad
  * @param          反馈的角度 rad
  * @param          记录的编码值 raw
  * @param          反馈的编码值 raw
  * @param          校准的步骤 完成一次 加一
  */
#define engineer_cali_clamp_min_judge(clamp2_min_ced, clamp_2_ced, step)                 \
    {																                     \
        if (clamp_cali_limit())															 \
        {																				 \
           /*clamp1_min_ced = clamp_1_ecd;  当只校准2号电机*/						         \
           clamp2_min_ced = clamp_2_ced;												 \
           (step)++;																     \
        }																			     \
    }
/**
* @brief          通过判断速度来判断夹具是否到达极限位置，这里只几率最大角度，最小角度为限位触发
* @param          对应轴的角速度，单位rad/s
* @param          计时时间，到达GIMBAL_CALI_STEP_TIME的时间后归零
* @param          记录的角度 rad
* @param          计算的角度 rad
* @param          校准的步骤 完成一次 加一
*/
#define engineer_cali_clamp_max_judge(time ,clamp_speed, clamp_max_angle , clamp_last_angle , step)    \
{																								                                                       \
    if (clamp_speed == 0)																	                                             \
    {																							                                                     \
		time++;																					                                                   \
		if (time > engineer_cali_time)									                                               	   \
		{																			                                                             \
			time = 0;													                                                               \
			clamp_max_angle = clamp_last_angle;		                                						               \
			(step)++;												                                                                 \
		}																				                                                           \
	}															                                                                       \
}

/**
* @brief          遥控器死区限制
*/

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

/**
  * @brief          工程校准控制，电机是raw控制，校准夹具的最大活动角度
  * @author         RM
  * @param[out]     yaw:发送yaw电机的原始值，会直接通过can 发送到电机
  * @param[out]     pitch:发送pitch电机的原始值，会直接通过can 发送到电机
  * @param[in]      gimbal_control_set:云台数据指针
  * @retval         none
  */
static void engineer_cali_control(fp32* clamp, fp32* translation, engineering_obtein_move_t* control_set);
/**
  * @brief          矿石兑换
  * @retval         none
  */
static void engineer_exchange_set_state(engineering_obtein_move_t* exchange_set_state, fp32* clamp, fp32* frc_x, fp32* frc_y);

/**
  * @brief          矿石兑换
  * @retval         none
  */
static void engineer_friction_Servo(engineering_obtein_move_t* Servos);
/**
  * @brief          工程行为状态机以及电机状态机设置
  * @param[out]     gimbal_mode_set: 数据指针
  * @retval         none
  */
void engineer_behaviour_mode_set(engineering_obtein_move_t* set_mode)
{
	if (set_mode == NULL)
	{
		return;
	}

	//校准行为，return 不会设置其他的模式
	if (engineer_behaviour == engineer_CALI && set_mode->engineer_cali.step != engineer_CALL_END_STEP)
	{
		return;
	}
	//如果外部使得校准步骤从0 变成 start，则进入校准模式
	if (set_mode->engineer_cali.step == engineer_CALI_START_STEP && !toe_is_error(DBUS_TOE))
	{
		engineer_behaviour = engineer_CALI;
		return;
	}
	//初始化模式判断是否到达极限位置，后续更新
	if (engineer_behaviour == engineer_INIT)
	{
		static int init_time = 0;
		static uint16_t init_stop_time = 0;
		init_time++;

		if (clamp_cali_limit())        //目前判断的夹具限位，具体待定
		{
			if (init_stop_time < clamp_init_stop_time)
			{
				init_stop_time++;
			}
			else
			{
				if (init_time < clamp_init_time)
				{
					init_time++;
				}
			}
		}

		if (init_stop_time < clamp_init_stop_time && init_time < clamp_init_time && !switch_is_down(set_mode->engineering_obtein_RC->rc.s[engineer_mode_channel0]) && !toe_is_error(DBUS_TOE))
		{
			return;
		}
		else
		{
			init_stop_time = 0;
			init_time = 0;
		}
	}

	//开关控制 工程状态
	if (switch_is_down(set_mode->engineering_obtein_RC->rc.s[engineer_mode_channel0]))          //876位，行为模式
	{
		engineer_behaviour = engineer_ZERO_FORCE;
	}
	else if (switch_is_mid(set_mode->engineering_obtein_RC->rc.s[engineer_mode_channel0]))
	{
		engineer_behaviour = engineer_RELATIVE_ANGLE;
	}
	else if (switch_is_up(set_mode->engineering_obtein_RC->rc.s[engineer_mode_channel0]))
	{
		set_mode->transmit_chassis.state_flag &= 0xBF;      //底盘姿态闭环，后续可能将添加到开关1
	}

	if (switch_is_down(set_mode->engineering_obtein_RC->rc.s[engineer_mode_channel1]))           //4,5位  工作模式
	{
		engineer_work = engineer_rescue;
		set_mode->transmit_chassis.state_flag &= 0xE7;
	}
	else if (switch_is_mid(set_mode->engineering_obtein_RC->rc.s[engineer_mode_channel1]))
	{
		engineer_work = engineer_obtein;
		set_mode->transmit_chassis.state_flag &= 0xEF;
	}
	else if (switch_is_up(set_mode->engineering_obtein_RC->rc.s[engineer_mode_channel1]))
	{
		engineer_work = engineer_exchange;
		set_mode->transmit_chassis.state_flag &= 0xF7;
	}

	if (toe_is_error(DBUS_TOE))
	{
		engineer_behaviour = engineer_ZERO_FORCE;
	}

	//判断进入init状态机

	static engineer_behaviour_e last_engineer_behaviour = engineer_ZERO_FORCE;
	if (last_engineer_behaviour == engineer_ZERO_FORCE && engineer_behaviour != engineer_ZERO_FORCE)
	{
		engineer_behaviour = engineer_INIT;
	}
	last_engineer_behaviour = engineer_behaviour;


	/************根据行为状态设置电机控制模式*************/
	if (engineer_behaviour == engineer_ZERO_FORCE)
	{
		set_mode->engineer_motor_mode = engineer_motor_raw;
		set_mode->transmit_chassis.state_flag &= 0x1F;
	}
	else if (engineer_behaviour == engineer_CALI)
	{
		set_mode->engineer_motor_mode = engineer_motor_raw;
		set_mode->transmit_chassis.state_flag &= 0x3F;
	}
	else if (engineer_behaviour == engineer_INIT)
	{
		set_mode->engineer_motor_mode = engineer_motor_raw;
		set_mode->transmit_chassis.state_flag &= 0x5F;
	}
	else if (engineer_behaviour == engineer_RELATIVE_ANGLE)
	{
		set_mode->engineer_motor_mode = engineer_motor_encode;
		set_mode->transmit_chassis.state_flag &= 0x7F;
	}
	else if (engineer_behaviour == engineer_MOTIONsLESS)
	{
		set_mode->engineer_motor_mode = engineer_motor_encode;
		set_mode->transmit_chassis.state_flag &= 0x9F;
	}

}

/**
  * @brief          工程行为控制，根据不同行为采用不同控制函数
  * @param[out]     add_yaw:设置的yaw角度增加值，单位 rad
  * @param[out]     add_pitch:设置的pitch角度增加值，单位 rad
  * @param[in]      gimbal_mode_set:云台数据指针
  * @retval         none
  */
void engineer_behaviour_control_set(fp32* translation, fp32* clamp, fp32* frc_x, fp32* frc_y, engineering_obtein_move_t* engineer_control_set)
{
	if (translation == NULL || clamp == NULL || frc_x == NULL || frc_y == NULL || engineer_control_set == NULL)
	{
		return;
	}

	if (engineer_behaviour == engineer_ZERO_FORCE)
	{
		*translation = 0.0f;
		*clamp = engineer_control_set->Clamp_Init_Angle;
		*frc_x = 0.0f;
		*frc_y = 0.0f;
	}
	if (engineer_behaviour == engineer_CALI)
	{
		engineer_cali_control(translation, clamp, engineer_control_set);
	}
	else if (engineer_behaviour == engineer_INIT)
	{
		if ((clamp_cali_limit()))
		{
			*clamp = engineer_control_set->Clamp_Init_Angle;
			*translation = 0.0f;                           //横向臂校准待
		}
		else
		{
			*clamp = engineer_control_set->Clamp_Init_Angle;
		}
	}

	else if (engineer_behaviour == engineer_RELATIVE_ANGLE)     //控制夹取相关
	{
		if ((engineer_work == engineer_obtein) || (engineer_work == engineer_rescue))                   //开关打到中取矿
		{
			engineering_obtein_set_state(engineer_control_set);

			engineering_obtein_set_change(translation, clamp, engineer_control_set);

			engineering_obtein_set_control(clamp, frc_x, frc_y, engineer_control_set);
		}
		else if (engineer_work == engineer_exchange)            //开关打到上兑换矿石
		{
			engineering_obtein_set_state(engineer_control_set);
			engineer_control_set->translation_set_mileage = 0.0f;  //横向臂保持不动
			//engineer_exchange_set_state(engineer_control_set, clamp, frc_x, frc_y);
			engineering_obtein_set_control(clamp, frc_x, frc_y, engineer_control_set);
			engineering_exchange_set_change(clamp, engineer_control_set);
		}

	}
	else if (engineer_behaviour == engineer_MOTIONsLESS)
	{
		*translation = 0;
		*clamp = engineer_control_set->Clamp_Init_Angle;
		*frc_x = 0.0f;
		*frc_y = 0.0f;
	}
	else return;
}


/**
  * @brief          底盘行为控制，根据不同行为采用不同控制函数
  * @param[out]     add_yaw:设置的yaw角度增加值，单位 rad
  * @param[out]     add_pitch:设置的pitch角度增加值，单位 rad
  * @param[in]      gimbal_mode_set:云台数据指针
  * @retval         none
  */

void chassis_rc_control_set(engineering_obtein_move_t* chassis_control_set)
{
	if (chassis_control_set == NULL )//|| !(engineer_work == engineer_rescue))
	{
		return;
	}
	
	int16_t vx_channel, vy_channel, wz_channel;
	fp32 vx_set_channel, vy_set_channel, wz_set_channel;
	
	//死区限制，因为遥控器可能存在差异 摇杆在中间，其值不为0     从这里取出RC的值    这里是遥控的通道
	rc_deadband_limit(chassis_control_set->engineering_obtein_RC->rc.ch[CHASSIS_X_CHANNEL], vx_channel, CHASSIS_RC_DEADLINE);
	rc_deadband_limit(chassis_control_set->engineering_obtein_RC->rc.ch[CHASSIS_Y_CHANNEL], vy_channel, CHASSIS_RC_DEADLINE);
	rc_deadband_limit(chassis_control_set->engineering_obtein_RC->rc.ch[CHASSIS_Z_CHANNEL], wz_channel, CHASSIS_RC_DEADLINE);

	vx_set_channel = (int8_t)(vx_channel / RC_TO_CHASSIS_SPEED_SEN);
	vy_set_channel = (int8_t)(vy_channel / RC_TO_CHASSIS_SPEED_SEN);
	wz_set_channel = (int8_t)(wz_channel / RC_TO_CHASSIS_SPEED_SEN) - (chassis_control_set->engineering_obtein_RC->mouse.x)/2.0f;    //6.6F

    
	//键盘控制    对应按键  
	if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_W)
	{
		if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_SHIFT)
		{
		  vx_set_channel = DASH_MAX_CHASSIS_SPEED_X;
         
	     }
		else{
		
		  vx_set_channel = NORMAL_MAX_CHASSIS_SPEED_X;
		}
	}
	else if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_S)
	{
		if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_SHIFT)
		{
			vx_set_channel =-DASH_MAX_CHASSIS_SPEED_X;
		}
		else
		{	
			vx_set_channel = -NORMAL_MAX_CHASSIS_SPEED_X;
		}
			
	}

	if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_A)
	{
		if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_SHIFT)
		{   
			vy_set_channel =-DASH_MAX_CHASSIS_SPEED_Y;
		
		}
		else
		{	
			vy_set_channel =-NORMAL_MAX_CHASSIS_SPEED_Y;
		}
			
	}
	else if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_D)
	{
		if (chassis_control_set->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_SHIFT)
		{
		    vy_set_channel =DASH_MAX_CHASSIS_SPEED_Y;
		
		}
		else
	 	{	
			vy_set_channel = NORMAL_MAX_CHASSIS_SPEED_Y;
		}
	}


if(chassis_control_set->can_chassis_A->button_limit_flg)
{
  chassis_control_set->transmit_chassis.x_speed = -vx_set_channel;  //底盘的头向陆地(视野开阔)
	chassis_control_set->transmit_chassis.y_speed = -vy_set_channel;
	chassis_control_set->transmit_chassis.z_speed = wz_set_channel;     //z轴方向始终不变
}
else{
	chassis_control_set->transmit_chassis.x_speed = vx_set_channel;   //底盘的头向夹具
	chassis_control_set->transmit_chassis.y_speed = vy_set_channel;
	chassis_control_set->transmit_chassis.z_speed = wz_set_channel;      //z轴方向始终不变
    }
}


/**
  * @brief          工程校准控制，电机是raw控制，校准夹具的最大活动角度
  * @author         RM
  * @param[out]     yaw:发送yaw电机的原始值，会直接通过can 发送到电机
  * @param[out]     pitch:发送pitch电机的原始值，会直接通过can 发送到电机
  * @param[in]      gimbal_control_set:云台数据指针
  * @retval         none
  */
static void engineer_cali_control(fp32* translation,fp32* clamp, engineering_obtein_move_t* control_set)
{
	if (clamp == NULL || control_set == NULL || translation == NULL)
	{
		return;
	}
	static uint16_t cali_time = 0;

	if (control_set->engineer_cali.step == clamp_CALL_MIN_STEP)
	{
		*clamp = engineer_cali_set;                         //先正转，触碰限位开关，只记录ecd
		*translation = 0;

		//判断数据，记录最小角度
		engineer_cali_clamp_min_judge(control_set->engineer_cali.clamp_min_ced,control_set->engineer_Clamp_turn_motor_data[1].motor_measure->ecd, control_set->engineer_cali.step);
	}
	else if (control_set->engineer_cali.step == clamp_CALL_MAX_STEP)
	{
		*clamp = -engineer_cali_set;                       //只记录最大角度
		*translation = 0;

		engineer_cali_clamp_max_judge(cali_time, control_set->engineer_Clamp_turn_motor_data[1].motor_measure->speed_rpm,
		control_set->engineer_cali.clamp_max_angle, control_set->clamp_turn_last_angle, control_set->engineer_cali.step);
	}
	else if (control_set->engineer_cali.step == engineer_CALL_END_STEP)
	{
		cali_time = 0;
	}
}


/**
  * @brief          矿石兑换
  * @retval         none
  */
static void engineer_exchange_set_state(engineering_obtein_move_t* exchange_set_state, fp32* clamp , fp32* frc_x, fp32* frc_y)
{
	if (exchange_set_state == NULL || clamp == NULL || frc_x == NULL || frc_y == NULL)
	{
		return;
	}
	 int16_t go_down_channel=0;
	 int8_t  go_down_mileages=0;
	//夹具  调整
	
				 if (exchange_set_state->engineering_obtein_RC->key.v & CLAMP_UP_KEY)   //夹具向下转动
				{	
				//exchange_set_state->engineer_obtein_state = engineering_Clamping_ready;			
				if (exchange_set_state->clamp_turn_last_angle <= exchange_set_state->clamp_max_angle+5)
				{
	              *clamp += 0.01f;
				}
				else
				{
			     return;
				}
	    	}
	     	else if (exchange_set_state->engineering_obtein_RC->key.v & CLAMP_DOWN_KEY)
	    	{
				//exchange_set_state->engineer_obtein_state = engineering_Clamping_ready;	
				if (exchange_set_state->clamp_turn_last_angle >= exchange_set_state->clamp_min_angle)
				{
					*clamp -= 0.01f;
			    }
				else
				{
				return;
				}
	    	}
	    //矿石 调整 方向
		if (exchange_set_state->engineering_obtein_RC->key.v & FRICTIONY_KEY)
		{
			if (exchange_set_state->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)         //y方向
        	{
			*frc_y = exchange_set_state->friction_motor_max_speed;
			}
		}
	    else if (exchange_set_state->engineering_obtein_RC->key.v & FRICTION_Y_KEY)
		{
			if (exchange_set_state->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)         //y方向
	        {
			*frc_y = -exchange_set_state->friction_motor_max_speed;
			}
		}
		else if (exchange_set_state->engineering_obtein_RC->key.v & FRICTIONX_KEY)
		{
			if (exchange_set_state->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)         //X方向
         	{
			*frc_x = exchange_set_state->friction_motor_max_speed;
			}
		}
		else if (exchange_set_state->engineering_obtein_RC->key.v & FRICTION_X_KEY)
		{
			if (exchange_set_state->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)         //X方向
        	{
			*frc_x = -exchange_set_state->friction_motor_max_speed;
			}
		}
		else
		{
		*frc_x=0;
		*frc_y=0;
		}
	if (exchange_set_state->engineering_obtein_RC->key.v & SAVE_OBTEIN_KEY)
	{
		exchange_set_state->fric_exchange = 1;
		
	}
	else
	{
		exchange_set_state->fric_exchange = 0;
	}
	//开始夹取，气缸工作
	if (exchange_set_state->engineering_obtein_RC->mouse.press_l)
	{		
		exchange_set_state->engineer_obtein_state = engineering_Clamping;
	}
	else if (exchange_set_state->engineering_obtein_RC->mouse.press_r)
	{
		exchange_set_state->engineer_obtein_state = engineering_No_Clamp;
	}
	else if(exchange_set_state->engineering_obtein_RC->key.v & RESET_KEY)
	{
		exchange_set_state->engineer_obtein_state = engineering_Clamping_init;
	}else return;

	//升降控制
	if (exchange_set_state->engineering_obtein_RC->key.v & GO_KEY)
	{
		//exchange_set_state->go_down_add_high_flag = 0;
		go_down_mileages = 35;	
	}
	else if (exchange_set_state->engineering_obtein_RC->key.v & DOWN_KEY)
	{
		//exchange_set_state->go_down_add_high_flag = 0;
		go_down_mileages= -35;	
	}
	else
	{
		go_down_mileages = 0;
	}

  //exchange_set_state->transmit_chassis.go_down_speed=go_down_mileages;
	//死区限制，因为遥控器可能存在差异 摇杆在中间，其值不为0     从这里取出RC的值    这里是遥控的通道
  //	rc_deadband_limit(exchange_set_state->engineering_obtein_RC->rc.ch[3], go_down_channel, CHASSIS_RC_DEADLINE);
	exchange_set_state->transmit_chassis.go_down_speed = (int8_t)(exchange_set_state->engineering_obtein_RC->rc.ch[3]/RC_TO_CHASSIS_SPEED_SEN)+go_down_mileages;
	//限制最高的高度
}

/**
  * @brief          摩擦轮舵机变换松紧
  * @retval         none
  */
static void engineer_friction_Servo(engineering_obtein_move_t* Servos)
{
    if(Servos->Servo_state==Servo_reset)
		{

    __HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, 0);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, 0);

		}
    else if(Servos->Servo_state==Servo_set)
		{    
    __HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, 0);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, 0);
		}
    else return;
			 
}


/************************************************the end**********************************************************/



